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techniques.  This  is  considered  important  for  undersea  operations  where  positioning 
systems  such  as  GPS  are  either  not  available  or  difficult  to  put  in  place.  There  are  several 
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I.  INTRODUCTION 


A,  MOTIVATION  FOR  THIS  WORK 

A  fundamental  requirement  for  unmanned  systems  is  the  ability  to  aoeurately 
estimate  position.  The  ubiquitous  Global  Position  System  (GPS)  is  used  for  a  wide 
variety  of  aerial,  surfaee  and  ground  vehieles,  but  it  has  limitations — the  signal  ean  be 
jammed  or  oceluded.  What  is  desired  is  a  robust  methodology  for  position  estimation  that 
is  not  dependent  upon  an  external  system  of  navigational  beaeons. 

One  alternative  is  terrain  aided  navigation  (TAN).  It  is  a  teehnique  that  uses 
onboard,  exteroeeptive  ranging  sensors  as  a  navigational  aid.  Seminal  work  developing 
TAN  methods  was  first  eompleted  in  the  terrain  eontour  matehing  (TERCOM)  algorithm 
employed  upon  cruise  missiles  in  the  1960s,  before  GPS  was  available.  While  the  advent 
of  GPS  alleviated  some  of  the  motivation  for  further  TAN  work,  it  remained  a  viable 
option  for  undersea  localization  since  GPS  signals  cannot  significantly  penetrate  the 
water  surface. 

Currently,  commercial  AUV  systems  rely  heavily  upon  a  costly,  high-grade 
inertial  navigation  system  (INS)  in  order  to  estimate  the  state  of  the  vehicle.  However, 
due  to  the  dead-reckoning  nature  of  INS  systems,  they  are  susceptible  to  drift  over  time. 
Unless  localized  by  some  other  means,  the  vehicle’s  positional  uncertainty  grows  without 
bound.  Typically,  this  growing  uncertainty  is  corrected  through  the  aid  of  either  a 
network  of  acoustic  ranging  transponders  or  resurfacing  for  a  GPS  fix.  Both  methods  are 
at  the  least  an  inconvenience,  and  at  most  are  unrealistic,  costly,  and  potentially  mission 
threatening.  TAN  presents  an  appealing  alternative  method  of  localization  for  an 
underwater  vessel  that  can  be  implemented  real-time  with  sensors  already  onboard. 

B,  ENABLING  TECHNOLOGIES 

While  TERCOM  was  first  implemented  in  cruise  missiles  in  the  1960s,  the  lack  of 

necessary  sensor  accuracy,  computational  power,  and  data  storage,  along  with  other 

challenges  associated  with  the  undersea  environment,  have  greatly  delayed  TAN 

implementation  in  the  undersea  domain  [1].  Eor  the  last  35  years,  the  world  has  seen 
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remarkable  improvements  in  eomputational  power  and  data  storage,  as  well  as  sensor 
aeouraey.  The  inereased  proeessing  power  and  data  storage  eapabilities  have  not  only 
signifieantly  enhaneed  the  aeeuraey  of  past,  proven  methods  of  TAN,  but  have  also 
enabled  the  implementation  of  newer,  more  eomputationally  expensive  algorithms. 
A  eomparison  of  different  methods  for  TAN  is  eovered  in  Seetion  D  as  well  as  in 
Chapter  IV. 

C.  PROBLEM  DESCRIPTION 

TAN,  unlike  the  related  field  of  simultaneous  loealization  and  mapping  (SLAM), 
requires  a  prior  map  of  the  region.  The  overarehing  goal  of  TAN  is  to  effeetively  use  the 
prior  terrain  map  in  eonjunetion  with  new  sensor  information  in  order  to  aid  in  the 
navigation  of  the  vehiele.  Therefore,  the  work  presented  in  this  thesis  ean  be  separated 
into  two  main  subjeet  areas:  Building  an  aeeurate  bathymetry  map  and  using  the  built 
map  as  a  navigational  aid. 

First,  a  feature  rieh  bathymetrie  map  must  be  built.  The  map  building  proeess 
requires  sonar  image  proeessing,  a  eoordinate  transformation  between  the  vehiele ’s  body- 
fixed  referenee  frame  and  a  global  frame,  and  a  possible  interpolation  of  the  data  points 
in  the  global  frame.  There  are  many  ehallenges  and  eonsiderations  that  must  be  made 
within  the  seope  of  building  an  aeeurate  bathymetry  map.  One  of  the  more  signifieant 
eoneerns  affeeting  map  aeeuraey  is  the  growing  positional  uneertainty  of  the  vehiele  as  a 
funetion  of  distanee.  Other  eonsiderations  inelude  speeifie  image  proeessing  teehniques 
and  threshold  seleetions.  For  example,  ineluding  the  sonar  response  from  a  large  fish  in 
the  map  would  not  be  a  useful  feature  for  subsequent  loealization.  Ensuring  abrupt 
ehanges  in  bottom  topography  are  ineorporated  to  the  bathymetry  map  would  be  very 
important. 

The  seeond  main  eomponent  of  this  work  is  implementing  a  method  of  TAN. 
TAN  requires  an  algorithm  that  ean  effeetively  and  aeeurately  loealize  the  position  of  the 
underwater  vehiele  based  upon  the  previously  built  bathymetrie  map.  There  are  several 
methods  that  ean  be  used  to  varying  degrees  of  sueeess  to  aeeomplish  this  task.  First,  a 
kinematies  motion  model  must  be  determined  for  the  AUV.  It  is  worth  noting  that  the 
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kinematics  for  AUVs  can  be  nonlinear,  though  they  are  often  approximately  well  by  a 
linearization  [2],  Similarly,  a  measurement  model  must  be  formulated  that  encapsulates 
both  the  measurements  and  measurement  noise.  The  exteroceptive  measurements  of  the 
terrain  are  often  highly  non-linear  as  a  result  of  the  multiple  peaks  and  valleys  associated 
with  the  terrain.  After  appropriately  modeling  the  vehicle  and  measurements,  the  TAN 
problem  requires  a  correlation,  or  similarity  measure,  of  the  sensor’s  current 
measurements  with  the  prior  bathymetry  map.  Some  feasible  similarity  metrics  include 
cross-correlation  (XCOR),  normalized  cross-correlation  (NXCOR),  mean  absolute 
difference  (MAD)  and  minimum  square  distance  (MSD)  [1].  Due  to  the  nonlinearity  of 
the  terrain,  it  was  expected  that  the  probability  distribution  resultant  from  the  correlation 
would  be  multi-modal.  TAN  filtering  methods  currently  being  researched  include 
Kalman-based  filtering  methods,  multi-modal  adaptive  estimation  techniques,  and  the  use 
sequential  Monte  Carlo  methods,  namely  point  mass  and  particle  filters  [1]. 

In  our  application  of  TAN  to  an  AUV,  the  vehicle  built  a  bathymetry  map  of  its 
environment  using  a  micro-bathymetry  swath  sonar  sensor.  The  sonar  data  was  collected 
onboard  the  AUV  vehicle  and  then  post-processed  in  a  MATLAB  environment  to  “build” 
a  bathymetry  map.  When  the  AUV  subsequently  traversed  the  same  terrain,  the  new 
sonar  sensor  data  it  was  receiving  was  correlated  with  the  existing  bathymetry  chart  in 
order  to  update  the  vehicle  position  estimate.  Using  only  sonar  data  correlations  (no  state 
information  from  the  vehicle),  this  can  be  an  extremely  expensive  computational  process. 
Therefore,  several  of  the  aforementioned  methods  that  effectively  fuse  the  knowledge  of 
the  previous  vehicle  state,  the  current  sonar  readings,  and  the  existing  bathymetry  chart 
were  explored  in  the  scope  of  this  thesis.  These  methods  include  the  Kalman  filter,  the 
extended  Kalman  filter  (EKF),  and  particle  filter.  A  further  glance  into  the  advantages 
and  disadvantages  of  each  of  these  three  methods  is  covered  in  Section  D,  and  detailed 
more  explicitly  in  Chapter  IV.  A  primary  motivation  behind  the  eventual  selection  of  the 
particle  filter  as  a  solution  to  the  TAN  problem  was  its  ability  to  accurately  estimate  the 
probability  distribution  of  the  vehicle  state,  even  when  the  distribution  is  multi-modal. 

This  thesis  is  the  first  to  address  TAN  on  a  small,  man-portable  AUV.  The  work 
is  novel  in  the  sense  that  it  works  with  a  bathymetry  sonar  sensor  capable  of  significantly 
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higher  resolution  than  most  bathymetry  sonar  systems  utilized  for  TAN.  Along  with 
eoneern  to  aeeommodate  the  higher  resolution  sensor,  initial  eonsideration  has  been  given 
to  image  proeessing  teehniques  and  base  map  generation  that  are  eritical  in  an  eventual 
real-time  TAN  implementation.  This  proeess  was  validated  through  simulation  on  a 
dataset  eolleeted  by  the  AUV. 

D,  RELATED  WORKS 

The  teehnieal  landseape  of  TAN  methods  is  only  now  beginning  to  reaeh 
maturity.  Until  reeently,  very  few  eommereial  systems  employed,  or  relied  signifieantly 
upon,  any  terrestrial  sensory  information  as  a  eomponent  of  their  navigation.  As  the  field 
has  developed,  the  TAN  method  of  ehoiee  has  shifted  slightly.  In  the  mid-  to  late- 1990s, 
TAN  methods  for  AUV’s  employing  Kalman  filtering  methods  were  primarily  researehed 
and  implemented  [3].  In  partieular,  due  to  the  aforementioned  nonlinearities  assoeiated 
with  TAN  for  an  AUV,  the  extended  Kalman  filter  (EKF)  was  a  favorable  method  of 
sensor  fusion  among  researehers.  In  the  early  2000s,  partiele  filters  (PF)  and  point  mass 
filters  (PMF)  were  reeognized  as  approaehes  well-suited  to  handle  non-linear  proeess  and 
measurement  models  as  well  as  multi-modal,  non-parametrie  noise  distributions.  These 
teehniques  were  seleeted,  in  part,  due  to  the  rapid  inereases  in  eomputer  proeessing 
power/eapabilities  as  PF  and  PMF  are  eomputationally  expensive  algorithms  [4].  Several 
papers  published  throughout  the  early  2000s  provide  empirieal  support  for  the  superiority 
of  the  PF  relative  to  Kalman-based  or  bateh-oriented  methods.  These  papers  inelude 
Gustafsson  [5]  in  2001,  Nordlund  [6]  in  2002,  and  Anonsen  and  Hallingstad  [7]  in  2006. 

Carreno  et  al.  [1]  provide  an  exeellent  survey  of  AUV-based  TAN  researeh. 
Importantly,  the  survey  paper  also  provides  the  Bayesian  estimation  framework  relied 
upon  throughout  the  TAN  field.  Bayesian  estimation  is  a  partieularly  useful  approaeh  in 
the  underwater  domain,  sinee  it  implieitly  aeeounts  for  the  mean  and  varianee  assoeiated 
with  the  pose  of  the  vehiele.  Approaehes  that  use  a  Bayesian  framework  inelude  Kalman 
filtering,  multi-modal  adaptive  estimation  and  sequential  Monte  Carlo  methods. 

The  partiele  filter  and  point  mass  filter  implementations  by  Anonsen  and 
Hallingstad  [7]  on  a  HUGIN  AUV  provided  very  promising  results  demonstrating  the 


4 


suitability  of  recursive  Bayesian,  sequential  Monte  Carlo  methods  to  TAN.  One  large 
issue  that  is  identified,  but  not  addressed  in  their  approach,  is  the  impact  of  the  terrain 
“usefulness.”  Intuitively,  fiat  terrain  presents  an  issue  when  attempting  to  correlate  the 
current  measurements  with  the  prior  map.  When  the  AUV  was  tested  in  suitable  terrain, 
both  methods  yield  positional  accuracy  comparable  to  their  prior  map  resolution  (10  m). 
However,  it  was  noted  that  attempts  to  navigation  in  poorly  suited  terrain  could  often  lead 
to  filter  divergence. 

In  2012,  Professors  Shane  Dektor  and  Stephen  Rock  of  Stanford  completed 
further  testing  at  Monterey  Bay  Aquarium  Research  Institute  (MBARI)  on  the  TAN  of  a 
Dorado-class  AUV  [8].  Similar  to  Teixeira  et  al.  [9]  they  sought  to  improve  the 
robustness  of  the  particle  filter  TAN  implementation.  Incorrect  convergence  over  time  in 
flat  terrain  was  determined  to  be  a  product  of  “overconfidence”  of  the  filter  in  estimating 
position  over  the  featureless  terrain.  In  order  to  account  for  the  overconfidence,  an 
additional  parameter  dependent  upon  terrain  suitability  was  incorporated  into  the 
correlation  stage  of  the  particle  filter  algorithm.  The  parameter  effectively  scales  the 
observed  correlation  based  upon  the  usefulness  of  the  terrain  being  observed,  thereby 
reducing  correlation  confidence  over  featureless  terrain  while  maintaining  the  strong 
correlation  and  convergence  of  the  filter  over  feature-rich  terrain. 

E.  THESIS  ORGANIZATION 

The  thesis  is  organized  as  follows:  Chapter  II  provides  a  description  of  the  AUV 
and  the  sonar  used  for  research.  After  the  system/sensor  descriptions,  the  paper  delves 
into  building  the  bathymetry  map  necessary  in  order  to  conduct  subsequent  TAN 
operations.  Only  after  the  prior  map  building  methodology  is  discussed  in  Chapter  III  is 
the  selection  of  a  TAN  method  discussed  in  Chapter  IV.  Finally,  a  dataset  is  collected  and 
the  methodologies  described  are  implemented.  The  results  and  conclusions  from  the 
testing  are  presented  in  Chapter  V. 
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II.  SYSTEM  DESCRIPTION 


A,  REMUS  AUV 

All  data  processing  and  experimental  work  was  eompleted  through  the  use  of  a 
modified  REMUS  100  AUV  (Figure  1)  supplied  by  the  Center  for  Autonomous  Vehiele 
Researeh  (CAVR)  at  the  Naval  Postgraduate  Sehool  (NPS).  The  REMUS  vehieles  are 
eurrently  designed  and  manufaetured  by  Kongsberg  Maritime 
(http://www.km.kongsberg.eom). 

The  REMUS  100  in  partieular  is  designed  for  operation  in  eoastal  areas  in  depths 
of  up  to  100  meters.  This  makes  the  REMUS  100  vehiele  versatile  in  a  variety  of  shallow 
water  mission  areas  ineluding,  from  [10]: 

•  Hydrographie  surveys 

•  Mine  eounter  measure  operations 

•  Harbor  security  operations 

•  Environmental  monitoring 

•  Debris  field  mapping 

•  Seareh  and  salvage  operations 

•  Seientific  sampling  and  mapping 
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Figure  1.  REMUS  100  AUV  onboard  CAVR’s  SeaFox  surface  vessel 


The  standard  REMUS  100  is  a  lightweight  and  compact  AUV,  weighing 
approximately  37  kg  and  having  a  diameter  of  19  cm.  Its  light  weight  and  manageable 
size  make  it  easily  deployable  and  recoverable  by  two-man  teams  with  small  boats.  The 
vehicle  is  modular  and  can  be  configured  to  employ  a  wide  variety  of  sensors  and 
systems.  This  includes: 

•  MSTL  side  scan  sonar 

•  Upward  and  downward  RDl  acoustic  Doppler  current  profiler  (ADCP) 
Doppler  velocity  log  (DVL) 

•  Undersea  acoustic  modem 

•  GPS 

•  Fore  and  aft  cross-body  tunnel  thrusters 

•  YSl-600  Conductivity  Temperature  and  Depth  (CTD)  sensor 

•  Optical  Backscatter  Sensor 

•  External  Power  Data  Interface 

When  at  the  surface,  the  REMUS  100  vehicle  can  obtain  position  fixes  using 
GPS.  When  underwater,  however,  the  vehicle  relies  on  a  long  baseline  (LBE),  ultrashort 
baseline  (USBL)  or  integrated  DVL/GPS/INS  Kearfott  SeaDeViL  navigation  solution. 
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The  SeaDeViL  system  has  an  advertised  navigational  uneertainty  of  0.5  pereent  distance 
traveled  circular  error  probable  rate  (CEPR)  [12],  Essentially,  this  means  that  the  actual 
position  of  the  AETV  will  be  within  a  circle  of  a  radius  quantified  by  0.5  percent  times  the 
distance  traveled.  The  LBE  system  has  a  navigational  accuracy  of  ±10  meters  and  range 
of  2000  meters.  The  USBE  system  has  a  navigational  accuracy  of  ±1  meter  and  range  of 
1500  meters  [12]. 

B,  BLUEVIEW  MB2250  SONAR 

As  mentioned  above,  the  REMUS  has  a  modular  external  payload  configuration. 
The  power  data  interface  provides  a  nominal  30  volts  with  an  Ethernet  and  serial 
connection  within  a  wet-mateable  connector.  This  permits  the  REMUS  AUV  to  mount 
forward-looking  sensor  packages. 

The  forward-looking  sensor  package  mounted  on  the  NPS  REMUS  AUV  for  the 
data  collected  in  this  thesis  is  equipped  with  a  BlueView  900  KHz  forward-looking  sonar 
and  a  BlueView  2250  KHz  downward-looking,  ultra-high  resolution  sonar.  Both  systems 
are  blazed  array  sonars.  A  blazed  array  sonar  system  employs  methods  similar  to 
echelette  diffraction  gratings  in  optics  in  order  to  turn  a  single  sonar  acoustic  signal  into  a 
swath  of  sound  beams  [13].  Each  beam  is  diffracted  at  a  different  frequency  and  therefore 
at  a  different  angle  relative  to  the  source.  The  size  and  shape  of  each  beam  is  dependent 
upon  the  frequency  band  of  the  original  sound  source  as  well  as  the  shape  of  the  stave 
diffracting  the  sound.  Eigure  2  shows  the  magnitude  and  direction  of  each  beam  of  a 
composite  blazed  array  sonar  along  with  the  frequency  range. 
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300  kHz 


600  kHz 


As  can  be  seen,  the  lower  frequencies  create  larger  beams  and  as  the  frequency 
increases,  the  beam  size  decreases.  The  frequency  to  spatial  angle  relationship  remains 
consistent  throughout  the  process.  Therefore,  once  the  sound  from  each  beam  refleets 
back  towards  the  sonar  system,  the  process  is  reversed  and  the  individual  beams  are 
merged  back  into  a  single  signal  [13].  Due  to  the  directionality  of  the  individual  beams,  a 
wide  swath  of  coverage  can  be  attained  with  a  high  level  of  accuracy.  For  these  reasons, 
most  bathymetry  mapping  missions  utilize  a  form  of  a  blazed  array  sonar  system. 

Table  1  provides  the  speeifications  of  the  2250  KHz  downward-looking,  ultra- 

high  resolution  sonar  used  as  the  primary  sensor  in  this  thesis. 
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Table  1.  BlueView  MB2250  sonar  specifications,  after  [14] 


Attribute 

Value 

Field  of  View 

45“  X  r 

Minimum  Range 

0.5  meters 

Maximum  Range 

10  meters 

Beam  Width 

Ux  1“ 

Number  of  Beams 

256 

Beam  Spacing 

0.18“ 

Time  Resolution 

0.39  inches  (0.01  m) 

Max  Update  Rate 

40  Hz 

Frequency 

2.25  MHz 
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III.  MAP  BUILDING 


The  first  step  in  the  TAN  proeess  is  the  development  of  a  base  referenee  map.  It  is 
this  map  that  is  compared  with  bathymetry  sensor  measurements  to  determine  the  best 
correlation  to  determine  the  best  position  estimate  for  the  AUV.  This  chapter  describes 
the  challenges  and  necessary  steps  for  building  the  base  bathymetry  map. 

A,  BATHYMETRY  MAP  BUILDING 

Constructing  an  accurate  bathymetry  map  involves  the  following  considerations. 

•  AUV  kinematics  and  dynamics  and  environmental  impact 

•  Limited  sonar  range 

•  Low  and  asymmetrical  frequency  of  sonar  pings 

•  Computational  power  constraints 

•  Data  storage  constraints 

•  Inherent  inaccuracies  associated  with  uncertain  INS  pose  estimations 

One  of  the  most  challenging  aspects  of  accurate  map  building  is  accounting  for 

the  positional  uncertainty  of  the  vehicle  collecting  the  micro-bathymetry  sonar  images. 
Any  navigational  aid  that  can  be  utilized  in  order  to  constrain  or  reduce  the  uncertainty  of 
the  AUV  is  desired.  This  includes  GPS,  LBL  and  USBL  systems. 

The  growing,  unconstrained  uncertainty  associated  with  the  INS  is  by  no  means 
nominal.  A  conventional  navigation  pattern  for  complete  sensor  coverage  is  often  called  a 
“navigate  by  rows,”  or  “lawnmower,”  mission.  In  the  case  of  the  REMUS  vehicle,  a 
typical  survey  area  may  be  approximately  400  meters  by  400  meters.  Through  setting  the 
desired  altitude  of  the  vehicle  at  9  meters,  and  given  that  the  beam  from  the  micro¬ 
bathymetry  sonar  is  45  degrees  wide,  the  sonar  swath  will  be  10.04  meters  wide  by  the 
time  it  reaches  the  seafloor.  Therefore,  through  using  a  spacing  of  10  meters  between 
each  row,  complete  sensor  coverage  can  be  attained  in  ideal  circumstances.  In  total,  the 
AUV  will  travel  approximately  16,800  meters.  Given  that  the  positional  uncertainty  is 
equal  to  0.5  percent  of  the  distance  traveled  without  the  use  of  any  external  navigational 
aids,  the  accumulated  uncertainty  by  the  end  of  the  mission  is  84  meters  CEPR. 
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Figure  3  provides  a  visual  representation  of  the  aeeumulation  of  uneertainty  as  a 
mission  progresses.  The  blue  dots  represent  the  mean  estimate  of  the  filter  and  the  red 
ellipses  represent  the  positional  uneertainty  associated  with  the  INS  navigation  solution. 
Note  that  the  positional  uncertainty  is  drawn  such  that  there  is  a  50  percent  chance  that 
the  true  position  of  the  AUV  is  somewhere  within  the  boundary. 

Mission  parameters  such  as  vehicle  altitude,  speed,  and  tightness  of  turns  should 
also  be  given  due  diligence.  A  higher  altitude  off  the  seafloor  inherently  provides 
increased  coverage  area  and  thus  lessens  the  needed  distance  traveled  by  the  AUV  in 
order  to  survey  a  specified  area.  The  altitude  is  limited  by  the  range  of  the  sonar  sensor 
(10  meters)  plus  a  slight  buffer  of  one  to  two  meters  in  order  to  ensure  abrupt  changes  in 
the  topography  do  not  cause  a  loss  of  bottom  coverage.  Similarly,  given  that  the  sonar 
system  pings  at  roughly  1  Hz,  additional  coverage  can  be  gained  by  decreasing  the  speed 
of  the  vehicle.  Lastly,  the  dynamics  and  kinematics  of  the  vehicle  should  be  kept  in  mind 
in  order  to  appropriately  set  realistic  mission  paths  (i.e.,  turns  wide  enough  that  the 
vehicle  is  capable  of  making,  or  to  set  the  minimum  speed  at  which  the  vehicle  should 
operate  in  order  to  maintain  its  depth  and  overall  controllability). 
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Figure  3.  REMUS  AUV  mission  route  with  uncertainty — first  navigate  rows 

objective 

Figure  4  clearly  quantifies  the  accumulation  of  navigational  uncertainty 
throughout  the  course  of  the  mission.  The  REMUS  vehicle  embarks  on  the  mission  with  a 
GPS  fix  providing  accuracy  to  within  three  meters.  Approximately  midway  through  the 
mission,  the  REMUS  vehicle  surfaces  again  in  order  to  obtain  another  GPS  fix.  The  GPS 
improves  the  INS  solution  and  again  confines  the  estimated  position  to  a  three-meter 
radius  area. 
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Figure  4.  Accumulation  of  REMUS  positional  uncertainty  with  GPS  update 

Upon  the  completion  of  the  mission,  a  final  GPS  fix  is  obtained  and  the  positional 
uncertainty  is  constrained  once  again.  It  is  clear  from  Figures  3  and  4  above  that  the 
vehicle  must  either  surface  periodically  for  a  GPS  fix  or  navigate  its  track  with  the  aid  of 
an  acoustic  navigation  system  in  order  to  maintain  a  reasonable  position  estimate. 
Alternatively,  the  micro-bathymetry  surveying  may  be  completed  using  a  surface  vessel. 
This  way,  the  vessel  would  have  persistent  GPS  coverage.  However,  this  method  has  its 
own  limitations.  For  example,  the  high  resolution  BlueView  MBE2250  sonar  used  in  our 
experiments  would  severely  constrain  the  operating  area  and  coverage  of  the  surface 
vessel  due  to  its  limited  range.  Typically,  a  sonar  operating  at  a  much  lower  frequency 
would  be  used  in  order  to  increase  the  sonar  range.  Using  a  lower  frequency,  however, 
negatively  impacts  the  sonar  image  resolution. 

B.  SONAR  IMAGE  PROCESSING 

Over  the  course  of  a  mission,  the  AUV  collects  sonar  images  from  its  onboard 
sonar  sensor.  In  a  real-time  implementation,  image-processing  techniques  are  applied 
onboard  the  AUV  to  provide  a  series  of  altitude  measurements.  For  this  thesis,  the 
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analysis  was  conducted  via  post-mission  processing.  Upon  completion  of  the  mission,  the 
raw  images  are  then  imported  from  the  vehicle  into  a  MATLAB  environment  in  order  to 
be  post-processed.  The  goal  of  the  sonar  image  processing  step  is  to  identify  and  extract 
information  regarding  the  topology  of  the  seafloor  surveyed. 

A  typical  raw  sonar  image  from  the  BlueView  MB2250  micro-bathymetry  sonar 
is  shown  in  Figure  5. 


Micro-Bathymetry  Sonar  Image 
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Figure  5.  Sample  micro-bathymetry  sonar  image 

The  intensity  values  of  the  image  vary  from  0  to  10000.  A  threshold  of  125  was 
determined  empirically  to  be  a  suitable  threshold  for  separating  noise  from  actual  bottom 
returns.  Thresholding  attempts  to  minimize  false  acceptances  of  noise  as  actual  bottom 
returns  while  also  minimizing  false  rejections,  where  valuable  bottom  information  may 
be  discarded  as  noise.  It  should  be  noted  here  that  the  thresholding  value  of  125  is  the 
result  of  an  empirical  evaluation  of  a  particular  data  set.  It  would  be  useful  to  include  an 
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autonomous  methodology  for  determining  a  filtering  threshold  regardless  of  the 
operational  environment  (i.e.,  sandy  or  muddy  ocean  floor).  An  adaptive  thresholding 
method  is  a  consideration  for  future  work.  For  the  dataset  collected  for  this  thesis,  a  max 
return  of  every  eleventh  column  can  be  located  in  the  image  and  compared  with  a 
threshold  value  of  125.  If  the  maximum  pixel  value  of  the  column  is  above  125,  it  is 
regarded  as  the  bottom  location  in  the  image.  Based  upon  sampling  every  eleventh 
column,  a  bottom  return  can  be  provided  approximately  every  0.25  meters.  A  map 
resolution  of  0.25  meters  was  determined  appropriate  in  seeking  to  accomplish  the 
objectives  set  in  this  thesis.  In  future  work,  if  determined  desirable,  the  resolution  could 
be  increased  to  the  fundamental  0.023052  meters/pixel  associated  with  the  sonar  images 
produced.  An  example  of  finding  the  max  return  of  each  eleventh  column  in  the  sonar 
image  is  depicted  in  Figure  6. 


Micro-Bathymetry  Sonar  Image 
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Figure  6.  Sample  sonar  image  with  max  returns 


18 


In  the  example  of  Figure  6,  the  four  red  pluses  to  the  right  of  the  visible  bottom 
returns  would  be  removed  by  the  threshold  from  the  colleetion  of  useful  bottom  data.  The 
final  set  of  data  points  colleeted  would  be  those  represented  by  the  red  pluses  in  Figure  7. 
Using  the  pixel  location  of  each  point  greater  than  the  threshold  along  with  the  associated 
conversion  of  0.023052  meters  per  pixel,  the  distance  in  meters  to  each  max  return  can  be 
computed  in  the  vehicle’s  local  frame. 


Micro-Bathymetry  Sonar  Image 
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Figure  7.  Sonar  image  with  max  returns  after  thresholding 


C.  TRANSFORMATION  TO  GLOBAL  FRAME 

Since  the  BlueView  MB2250  sonar  is  rigidly  attached  to  the  underside  of  the 
REMUS  100  AUV,  each  sonar  image  is  captured  relative  to  the  body’s  pose.  In  order  for 
the  information  to  be  useful,  the  bottom  returns  must  all  be  represented  in  the  same  global 
map.  The  rotation  transformation  converts  the  identified  bottom  locations  in  the  body 
frame  to  bottom  locations  in  the  local  tangent  plane  (LTP)  and  converted  into  the  global 
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frame.  First,  the  vehiele’s  pose  is  taken  into  aceount.  The  loeation  of  the  bottom  returns  is 
affeeted  by  both  the  vehiele’s  roll,  piteh  and  yaw,  represented  by  (j),  0,  and  y/ 
respectively.  The  Euler  angle  rotation  matrix  that  rotates  the  information  from  the  body  to 
LTP  frame  is  shown  in  equations  (1)  and  (2). 
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All  bottom  returns  from  the  micro-bathymetry  sonar  are  collected  in  the  AUV’s  x, 
y,  and  z  directions,  using  the  conventional  body-fixed  coordinate  system  abiding  by  the 
right  hand  rule.  In  order  to  express  all  data  points  as  relative  to  the  ocean  surface,  the 
depth  of  the  AUV  must  be  added  to  the  new  z  direction  of  the  data  points.  The  origin  of 
the  LTP  is  defined  as  a  point  on  the  surface  of  the  ocean,  and  therefore  a  transformation 
between  the  vehicle’s  body-fixed  frame  and  the  LTP  necessitates  both  a  rotation  and 
translation  of  the  reference  frame.  The  rotation  is  determined  by  the  vehicle  Euler  angles 
while  the  translation  is  determined  by  vehicle  depth  and  an  arbitrary  x,  y  origin.  An 
example  of  an  AUV  on  a  bathymetry  mapping  mission  is  shown  in  Ligure  8. 
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Figure  8.  AUV  with  visible  bathymetry  sonar  overlay,  after  [15] 

In  practice,  the  body-fixed  coordinate  frame  would  originate  at  the  vehicles  center 
of  gravity.  The  positioning  of  the  coordinate  frame  in  Figure  8  serves  solely  as  a  visual 
guide  for  the  reader.  Figure  9  depicts  the  very  same  image  as  Figure  8,  but  with  red 
crosses  symbolizing  the  maximum  sonar  returns  associated  with  the  seafloor. 


Figure  9.  AUV  with  visible  bathymetry  sonar  overlay  and  maximum  bottom 

returns,  after  [15] 
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The  coordinates  of  each  red  cross  is  first  expressed  relative  to  the  vehicles  body 
frame.  After  rotation  and  translation  to  the  LTP,  the  same  sonar  returns  seen  in  Figure  9 
would  look  approximately  like  the  data  points  plotted  in  Figure  10. 


Bjthyrrwty  PonlCI«ue 


Figure  10.  Example  sonar  bottom  returns  expressed  in  LTP 


Since  the  AUV  mission  conducted  for  this  project  typically  did  not  transverse 
more  than  several  hundred  meters  across  the  seafloor  in  any  direction,  the  LTP  reference 
frame  supports  an  acceptable  representation  of  the  data.  Nonetheless,  the  points  are  also 
available  represented  in  the  global  frame  through  latitude  and  longitude.  The 
transformation  from  the  LTP  frame  to  the  global  frame  was  completed  using  MATLAB’s 
Map  Toolbox. 

D,  CONSTRUCTING  THE  BATHYMETRY  MAP 

Once  the  entire  set  of  data  for  the  mission  was  transformed  into  the  global 
reference  frame  in  latitude  and  longitude,  all  of  the  points  can  be  displayed  coherently  as 
a  point  cloud  in  3D  space.  An  example  of  several  successive  data  points  being  displayed 
simultaneously  in  the  LTP  frame  is  shown  in  Figure  1 1 . 
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Bathymetry  Point  Cloud 


LTP  Frame  -  West  (m) 

Figure  1 1 .  Several  successive  sonar  pings  displayed  together  in  the  LTP  frame 

During  the  correlation  steps  a  sonar  ping  is  rotated  and  translated  into  the  LTP 
and  compared  with  the  base  bathymetry  map.  The  base  map  will  likely  not  have  complete 
coverage  due  to  limited  sonar  resolution,  AUV  path  planning  and  following  constraints, 
and/or  a  slow  sonar  ping  frequency  among  other  reasons.  A  lack  of  complete  coverage 
may  result  in  an  inability  to  make  a  comparison  between  the  sonar  ping  and  the  base  map 
as  the  sonar  ping  may  ensonify  an  empty  region  in  the  base  map.  There  are  a  number  of 
possible  approaches  for  handling  the  sparse  data;  however,  this  thesis  provides  complete 
coverage  to  a  desired  resolution  using  linear  interpolation.  The  linear  interpolation  was 
performed  in  the  MATLAB  environment  with  the  built  in  TriScatteredlnterp  function. 
The  resultant  surface  passes  through  every  data  point  in  the  set  and  linearly  interpolates 
between  neighboring  data  points.  A  basic  linear  interpolation  may  not  best  represent  the 
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underlying  topography  and  further  research  may  be  done  in  this  area  to  identify  a  better- 
suited  method  of  data  interpolation.  The  linear  interpolation  of  the  sonar  pings  displayed 
in  Figure  1 1  is  exhibited  in  Figure  12. 


Figure  12.  The  linear  interpolation  of  the  sonar  pings  from  Figure  1 1 


The  same  linear  interpolation  is  applied  to  all  data  points  collected  within  the 
survey  area  of  the  REMUS  AUV.  As  one  would  assume,  the  linear  interpolation  better 
approximates  the  underlying  topography  in  regions  with  a  higher  density  of  data  points 
and  is  less  accurate  the  sparser  the  data  is. 
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IV.  TERRAIN  AIDED  NAVIGATION 


This  chapter  addresses  an  optimal  estimation  approaeh  to  TAN.  The  goal  is  to  use 
the  prior  bathymetry  map  from  Chapter  III  as  an  aid  to  vehiele  navigation.  Therefore,  the 
ehapter  starts  with  identifying  a  proeess  and  measurement  model  for  the  vehicle.  The 
proeess  and  measurement  model  will  be  vital  in  the  latter  steps  of  filtering  the  positional 
estimation  of  the  vehiele.  Next,  the  eorrelation  methods  that  quantify  the  similarity 
between  the  prior  map  and  eaeh  new  sonar  ping  are  discussed  and  a  method  is  seleeted. 
The  eorrelation  method  eoineides  with  the  measurement  model.  Finally,  the  prior  three 
eomponents  work  together  within  the  domain  of  a  reeursive  Bayesian  filtering  method  in 
order  to  estimate  the  vehiele  state.  Both  Kalman-based  filters  and  partiele  filters  are 
diseussed,  and  advantages  /  disadvantages  for  eaeh  are  noted.  Consistent  with  reeent 
literature,  and  taking  into  eonsideration  the  nonlinearities  and  multi-modalities  inherent 
to  the  TAN  problem,  a  partiele  filter  was  ehosen  to  be  implemented. 


A,  PROBLEM  STATEMENT 
1.  Process  Model 

A  generalized  proeess  model  for  an  AUV  is  shown  in  equation  3,  first  proposed 

by  [8]. 

=  (3) 

where  is  the  vehiele ’s  x,  y,  z  position  in  the  LTP  and  is  representative  of  INS 
noise.  The  INS  noise  is  assumed  to  be  a  zero-mean,  white  noise. 


2,  Measurement  Model 

The  sonar  and  map  models  are  shown  in  Equations  4  and  5  respeetively  [8].  The 
map  and  sonar  errors,  e.  ^  and  are  treated  separately.  Therefore,  the  sensor  model 

uses  the  difference  between  the  true  terrain  for  the  ping  at  time  k  and  the  measured 
altitude  for  the  ping  at  time  k  along  with  a  range  dependent  error,  ,  in  order  to 
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determine  the  range  j,.  ^  .  The  map  model  essentially  states  that  the  true  terrain  range 
A  )  .  differs  from  the  expeeted  range  A  ).  by  an  error  . 

yi,k=z,-h{x,).+e,^,  (4) 

h{x,).=h{x,).+v^^^  (5) 

•  z,,  is  the  veetor  of  the  measured  altitudes  at  eaeh  sonar  ping. 

•  y.  ^  designates  the  ranges  assoeiated  with  the  i“'  ping  at  time  step  k 

•  h[x^ ).  is  the  true  terrain  range  for  the  i"'  ping  at  time  k 

•  /?(x;i).  range  from  a  priori  map 

•  ^i,k  range  dependent  error  e,,  ~n(0,  ) 

•  ^n.ap  map  error  ~  TV  ( 0,  ) 

Sinee  the  position  of  the  AUV  is  not  known  preeisely  and  the  map  has  inherent 
errors,  is  not  known.  Therefore,  effeetively  our  goal  with  TAN  is  to  minimize  the 

argument  ^  by  varying  ).  throughout  the  seareh  area. 

3.  Correlation  Method 

At  eaeh  time  step  in  the  recursive  algorithm,  the  altitude  data  from  the  micro¬ 
bathymetry  sensor  is  compared  to  the  prior  bathymetry  map  at  several  locations.  A 
correlation  technique  is  necessary  between  the  sensor  data  and  the  bathymetry  map  to 
quantify  the  similarity  between  each  such  that  the  best  correlation  is  selected  as  the  most 
probable  location  of  the  AUV.  The  correlation  method  should  be  robust  across  varying 
levels  of  depth  as  well  as  to  sensor  noise.  Overall,  the  difficulties  associated  with  map 
correlation  are  largely  consistent  with  those  of  building  the  map  originally.  One  of  the 
significant  difficulties  associated  with  TAN  is  the  low  and  asymmetric  frequency  of  ping 
information. 
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Several  different  methods  of  eorrelation  were  tested  ineluding  eross-eorrelation 
(XCOR)  [16],  normalized  eross-eorrelation  (NXCOR)  [16],  mean  absolute  differenee 
(MAD)  [1],  a  normalized  minimum  absolute  differenee,  and  a  mean  square  differenee 
(MSD)  [1], 


NXCOR{x,) 


XCOR(x,)  =  ^±(z,,-h,(x,)) 

i=\ 

i=\ 


(6) 

(V) 

(8) 

(9) 


In  eomparison  testing  with  eaeh  method,  NXCOR,  MAD,  and  MSD  all  performed 
similarly  well.  XCOR  was  determined  to  be  ill-suited  to  the  problem  as  the  eorrelation 
performed  is  not  seale  invariant,  meaning  the  aetual  depth  values  from  the  prior  map 
influenoe  the  result  of  the  eorrelation.  This  is  dangerous,  as  the  eorrelation  value  between 
a  sonar  ping  and  itself  may  actually  be  poorer  than  a  correlation  between  the  same  ping 
and  an  arbitrarily  high  valued  set  of  sonar  data.  NXCOR  successfully  addresses  this 
undesired  dependence  upon  scale.  The  main  downfall  of  NXCOR,  however,  was  that  the 
implementation  only  recovers  Cartesian  shifts  in  data.  The  MAD  and  MSD 
implementations,  on  the  other  hand,  can  recover  rotation  shifts  as  well  as  translational 
shifts.  Both  MAD  and  MSD  performed  similarly  well,  but  due  to  the  nature  of  squaring 
the  differences,  MSD  exaggerates  the  peaks  and  valleys  of  the  correlation  matching. 
Therefore,  MAD  was  chosen  in  order  to  avoid  this  sort  of  “overconfidence”  in  the 
correlation  result. 


The  output  of  the  correlation  step  is  a  matrix  equivalent  to  the  size  of  the  area  of 

uncertainty  searched.  In  the  case  of  the  MAD  and  MSD  implementations,  each  cell’s 

value  is  the  result  of  the  correlation  between  the  new  ping  and  the  prior  map  at  that 

location.  Through  performing  an  element-wise  inversion  of  each  cell  and  then 

normalizing  the  whole  matrix  so  that  the  sum  of  the  matrix  is  equal  to  1,  a  probability 

density  function  describing  the  probability  of  sensing  the  current  ping  information  given 
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the  AUV  position  in  the  map  is  ealculated.  This  probability  density  funetion  is  key  to  the 
measurement  update  steps  in  the  subsequent  seetions. 

4.  Bayesian  Methods 

As  previously  stated,  given  the  dead  reekoning  nature  of  navigating  by  INS,  there 
is  a  growing  uneertainty  assoeiated  with  the  vehiele’s  pose  over  time.  Clearly  then,  the 
TAN  problem  is  best  expressed  through  a  stoehastie  proeess.  Thus,  the  used  in  the 

proeess  model  of  part  1  is  not  a  single,  preeise  x,  y,  z  loeation,  but  a  stoehastie  variable 
representing  the  estimate  of  the  position.  The  well-known  Bayesian  filter  represents  a 
broad  framework  for  whieh  to  estimate  the  a  posteriori  probability  distribution  of  the 
vehiele’s  state  given  a  proeess  model,  measurement  model,  and  the  a  priori  probability 
distribution  of  the  state.  The  Bayesian  filter  is  represented  in  equations  10  and  11  as 
eonsisting  of  a  predietion  and  eorrection  step.  The  prediction  determines  the  probability 
of  the  vehicle  being  at  state  given  all  of  the  previous  measurements.  This  is  done 

using  the  state  transition  model,  represented  by  |  ,  and  the  a  priori 

probability  distribution  p{x^_^  The  correction  step  then  uses  the  result  of  the 

prediction  step,  along  with  the  current  measurement  probability  given  the  current  state, 
represented  by  p{^y,^\x^) ,  and  the  probability  of  the  measurement  given  all  the  previous 

measurements,  represented  by  |  D^_j ) . 

PREDICTION  :p{x,  \D,_j)  =  ^p{x,  \x,_j,u,)p{x,_j\D,_j)  (10) 

CORRECTION  :p{x,\DA=  P^yk\Xk)  p{x^\D^_^) 

pyTk  \Dk-i) 

p{^Xi^_j)  Probability  of  robot  at  certain  state  (pose)  at  time  step  k-\ 

P{^k\  ^k-i ’  ^k )  State  transition  probability  (motion  model) 

P^Pk  I  )  Measurement  probability  -  Probability  of  observing  when  at 
state 
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Z)^  =  { j,- ;  i  =  1, k]  =  {y. :  i  =  1, k]  Set  of  all  measurements  up  until  time  k 

The  Bayesian  filter  above  provides  the  general  framework  for  whieh  state 
estimation  oecurs.  The  Bayesian  filter  eannot  be  used  direetly;  however,  as  there  is  not  an 
analytieal  solution  to  the  equations.  Instead,  recursive  solutions  to  the  Bayesian  filter, 
such  as  the  Kalman  filter  and  particle  filter,  are  potential  methods  for  solving  the 
Bayesian  estimation  problem. 

B,  EVALUATION  OF  RECURSIVE  BAYESIAN  ESTIMATION  METHODS 

The  recursive  Bayesian  estimation  methods  that  were  evaluated  in  the  context  of 
this  thesis  were  the  Kalman-based  methods  as  well  as  particle  filtering.  First,  a  brief 
summarization  of  the  Kalman  filter  is  provided. 


The  Kalman  filter  relies  upon  a  linear  system  of  differential  equations,  typically 
expressed  in  state-space  format  as  in  equations  12  and  13,  from  [17]. 


x,=Ax,_j+Bu,_j  +  w,_j 

(12) 

yt  =Hx,+v, 

(13) 

Time  update  (prediction) 

x'k  =  Ax,_j  +  Bu,_, 

(14) 

f;  =  af,_,a^  +  q 

(15) 

Measurement  Update  (correction) 

(16) 

(17) 

P,=(I-K,H)P- 

(18) 

where; 


— A  posteriori  error  covariance  matrix 
Z^“  — A  priori  error  covariance  matrix 
Q — Process  noise  covariance  matrix 
R  — Measurement  noise  covariance  matrix 
K  — Kalman  gain 

— Zero  mean,  white  process  noise  ~  A(0,Q) 

— Zero  mean,  white  measurement  noise  ~  A(0,/?) 

As  partially  evidenced  by  its  formulation,  the  Kalman  filter  requires  linear  and 
Gaussian  assumptions.  The  EKF  has  a  largely  similar  representation;  however,  it  is  able 

29 


to  complete  the  predietion  step  of  the  filter  using  a  nonlinear  set  of  differential  equations. 
Therefore,  the  EKF  is  typieally  a  better  estimator  than  the  Kalman  filter  for  problems 
governed  by  a  nonlinear  process  and/or  measurement  model.  Nonetheless,  the  EKF  still 
linearizes  about  the  eurrent  mean  and  eovarianee  at  eaeh  time  instanee  and  requires  the 
same  Gaussian  assumptions  as  the  ordinary  Kalman  filter  [17]. 

The  partiele  filter  is  a  different  form  of  reeursive  Bayesian  filtering.  The  particle 
filter  diseretizes  the  continuous  probability  density  funetions  assoeiated  with  Bayesian 
filter  through  a  large  set  of  particles  [1],  [18].  Eaeh  particle  represents  a  potential  state. 
The  particles  are  distributed  roughly  according  to  the  a  priori  and  a  posteriori  probability 
density  functions.  As  each  particle  represents  a  potential  state  of  the  vehicle,  each  particle 
is  passed  through  a  recursion  of  propagation  (according  to  the  process  model)  and 
measurement  updates.  The  algorithm  for  a  commonly  used  particle  filtering  method 
ealled  a  sequential  importance  resampling  (SIR)  partiele  filter,  is  shown  in  Eigure  13. 

Ij  Initialize  -  Approximate  the  a  priori 
probability  by  a  discrete  number  of  equally 
freighted  particles 

2)  Collect  new  obsers'ation  /  measurement 

3)  Compute  importance  weight  according 
to  the  probability  of  obserx-ation  given 
particle's  state 

4)  Normalize  particle  weights  *  •  |  I 

5)  Resample  fi-om  particles  according  to  | 

weights  (each  new  particle  is  equally- 
weighted) 

6)  Propagate  each  particle  to  next  time 
instance  using  process  model 

7)  Repeat  steps  2-6 

•• 

Eigure  13.  SIR  particle  filter  algorithm,  after  [19] 

The  initialization  of  the  partiele  filter  is  essentially  a  discretization  of  the 
probability  density  function  at  that  time.  In  the  ease  of  the  REMUS  vehicle,  an 


30 


initialization  of  the  particle  filter  would  be  performed  upon  diving  and  would  be  based 
upon  the  uncertainty  associated  with  the  GPS  fix  just  prior  to  diving.  The  next  main  step 
in  the  algorithm  is  related  to  the  measurement  update.  The  measurement  update  is 
performed  by  collecting  the  new  measurement  and  weighting  each  particle  according  to 
the  probability  of  being  at  that  particular  state  given  the  new  measurement.  The  weights 
for  all  particles  are  then  normalized.  Next  is  the  resampling  step.  The  same  number  of 
particles  are  drawn  from  the  weighted  particles,  but  are  once  again  equally  weighted. 
Once  the  particles  have  been  resampled,  the  process  model  is  used  to  propagate  the 
particles  forward  to  the  next  time  instance.  The  filter  then  continues  on  to  collect  another 
measurement,  weight  the  particles  accordingly,  resample,  and  propagate  the  particles  for 
each  time  step.  These  steps  can  be  done  in  real-time  as  the  measurements/observations 
are  being  received.  If  there  are  no  measurements  at  a  given  time  instance,  the  particles  are 
still  propagated  with  the  process  model  until  a  new  measurement  is  available. 

One  of  the  most  advantageous  aspects  of  particle  filtering,  especially  with 
consideration  to  TAN  for  an  AUV,  is  that  it  does  not  make  any  assumptions  on  the 
system  model.  While  even  the  EKF  makes  certain  linear  and  Gaussian  assumptions, 
particle  filter  implementations  do  not.  With  a  sufficient  number  of  particles,  nonlinear 
and  multi-modal  systems  models  can  still  be  well-approximated.  The  main  disadvantage 
of  particle  filters  in  comparison  with  Kalman-based  filters  is  the  significant  increase  in 
required  computational  processing  power.  Increased  processing  power  is  needed  for 
particle  filtering  as  typically  1,000-10,000  particles  are  required  for  an  accurate 
approximation  to  the  state  probability  density  function  [1].  Each  particle  must  be 
propagated  and  updated  at  each  time  instance,  and  therefore  computational  demand  can 
be  significant. 

C.  SELECTION  AND  APPLICATION  OF  METHOD 

Both  Kalman-based  methods  and  particle  filters  rely  partially  on  measurement 
updates.  In  terms  of  AUV  TAN,  the  measurements  are  the  depth  of  multiple  points  on  the 
seafloor  beneath  the  vehicle.  As  terrain  in  general  can  vary  greatly,  with  various 
mountains,  valleys,  peaks  and  dips,  the  measurements  collected  are  highly  nonlinear. 
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With  such  nonlinearity  associated  with  the  measurements,  it  is  typically  unclear  exactly 
where  within  a  region  of  terrain  any  single  measurement  may  have  come  from.  In  other 
words,  when  correlating  the  measurements  collected  from  a  single  ping  with  a  prior  map, 
especially  given  sensor  and  map  error,  there  are  likely  multiple  areas  within  the  region 
that  seem  to  correlate  well.  Over  time,  however,  one  such  positional  estimate  will 
continue  to  correlate  well  while  the  others  will  not.  It  is  due  to  the  nonlinearity  associated 
with  terrain  measurements  and  the  resultant  multinomial  probability  distributions  that 
particle  filtering  was  implemented  vice  a  Kalman-based  method.  An  example  of  the 
probability  density  function  output  from  the  correlation,  or  measurement  update  step,  has 
been  taken  from  a  dataset  described  in  the  Chapter  V  results  and  is  shown  in  Figure  14. 
Clearly,  the  distribution  in  the  example  of  Figure  14  is  binomial.  With  the  potential  for 
such  multinomial  distributions  in  mind,  the  particle  filter  was  the  preferential  choice  for 
filtering. 
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Figure  14.  Probability  of  measurement  given  state — />(  J*  |  ) 
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The  sequential  importanee  sampling  (SIS)  partiele  filter  formed  the  foundation  for 
most  partiele  filters  developed  since  [18].  The  SIR  particle  filter  is  a  later  derivative  of 
the  SIS  particle  filter  that  differs  in  one  key  aspect;  the  resampling  step.  Through 
resampling,  the  particle  fdter  can  avoid  a  documented  degeneracy  phenomenon,  where 
after  a  few  iterations  all  particles  have  negligible  weights  except  for  one.  Resampling 
essentially  eradicates  particles  with  small  weights  and  instead  concentrates  on  particles 
with  larger  weights.  Additionally,  the  SIR  particle  filter  requires  minimal  assumptions 
beyond  the  knowledge  of  the  state  dynamics  and  measurement  functions  [18].  The 
simplicity,  popularity  [1],  and  avoidance  of  the  degeneracy  phenomenon  were  key 
influencing  factors  in  the  decision  implement  the  SIR  particle  filter  as  a  TAN  filtering 
method. 
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V.  EXPERIMENTATION 


A,  DATA  SET  COLLECTION— SEATEST  II 

The  primary  source  of  data  used  for  this  thesis  originates  from  cooperative 
experimentation  entitled  “SEATEST  IT”  The  mission  was  conducted  as  a  part  of  a  joint 
experiment  between  CAVR  at  NFS  and  the  NASA  Johnson  Space  Center  (JSC).  The 
overarching  goal  of  the  collaboration  with  the  JSC  was  to  quantify  the  effects  of 
autonomy  on  mission  effectiveness,  efficiency,  and  safety  for  joint  robot-human 
operations  [20]. 

The  platform  from  which  the  cooperative  experimentation  took  place  is  the 
Aquarius  Underwater  Research  Station  located  in  Islamorada,  Elorida.  The  Aquarius 
underwater  research  habitat  is  the  only  operational  underwater  habitat  in  the  world  and  is 
operated  by  Elorida  International  University  (EIU).  SEATEST  II  is  similar  to  a  series  of 
exercises  collectively  known  as  NASA  Extreme  Environment  Mission  Operations 
(NEEMO)  where  astronauts  train  in  the  undersea  habitat  as  an  analogue  to  space 
operations.  These  “aquanauts”  may  be  station  at  Aquarius  for  as  long  as  a  month  at  a 
time.  The  area  of  operations  is  visually  represented  by  the  Google  Earth  screenshot  in 
Eigure  15. 
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Figure  15.  Aquarius  Research  Station — Coast  of  the  Florida  Keys,  from  [21] 


CAVR  contributed  to  the  research  effort  with  two  REMUS  100  AUVs  and  a 
SeaBotix  vLBVSOO  Tethered,  Flovering  AUV  (TFIAUS)  alongside  personnel  with 
advanced  technical  backgrounds  and  aligned  research  objectives  headed  by  Dr.  Doug 
Homer  and  Dr.  Noel  Du  Toil.  The  REMUS  AUV  is  pictured  in  Figure  16  along  with 
Aquanauts  stationed  at  the  Aquarius  habitat  (seen  in  background). 
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Figure  16.  Aquanauts  with  the  NFS  REMUS  AUV  at  the  Aquarius  Habitat, 

from  [20] 

A  research  emphasis  was  placed  on  expanding  unmanned  system  autonomy. 
Autonomy  enables  the  vehicle,  through  exteroceptive  sensing,  to  make  intelligent 
navigational  decisions  such  as  obstacle  avoidance  and  navigation  in  cluttered,  dynamic 
environments.  Underwater  operations  are  of  particular  interest  to  NASA  and  CAVR 
research  since  these  operations  require  unmanned  system  autonomy.  The  underwater 
domain  not  only  necessitates  the  use  of  alternate  means  of  positioning  and  navigation,  as 
it  is  void  of  a  ubiquitous  positioning  signal  such  as  GPS,  but  also  necessitates  an  alternate 
means  of  sensing  and  communication.  Overall,  the  underwater  domain  presents  a 
particularly  unique  and  challenging  environment  for  the  operation  of  unmanned  systems 
and  as  such. 

The  missions  conducted  during  SEATEST  11  combined  autonomous  mapping  and 
navigation,  multi-vehicle  (heterogeneous)  collaboration  and  information  sharing,  joint 
robot-diver  operations,  and  persistent  robotic  operations.  The  role  of  the  NFS  REMUS 
vehicles  was  to  survey  a  nearby  area  with  a  BlueView  MBE  2250  micro-bathymetry 
sensor.  From  the  surveyed  sonar  data,  an  accurate  bathymetry  map  could  be  built.  The 
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bathymetry  map  could  then  be  used  to  identify  an  area  of  interest  for  subsampling  by  the 
REMUS  or  an  aquanaut,  or  as  a  navigation  aid  to  another  AUV,  such  as  CAVR’s 
SeaBotix  vehicle. 

B.  EXPERIMENTAL  RESULTS  WITH  REMUS  AUV 

1,  Bathymetry  Map  Building 

Over  the  course  of  a  particular  REMUS  mission  collected  during  SEATEST  II,  a 
total  of  3,046  sonar  images  were  captured  along  with  the  estimated  vehicle  state  at  each 
instance.  Of  the  3,046  sonar  images,  2,767  of  them  were  within  range  of  the  seafloor  and 
thus  contained  usable  bathymetry  information.  From  those  2,767  images,  a  total  of 
56,491  data  points  were  captured.  The  total  distance  traveled  during  the  mission  was 
approximately  6,250  meters.  Figure  17  provides  a  perspective  on  the  location  of  the 
SEATEST  II  mission  relative  to  the  Aquarius  underwater  habitat  in  Islamorada,  EL.  The 
figure  was  composed  using  the  data  collected  during  the  mission  and  importing  into 
Google  Earth  as  a  KML  file.  Each  yellow  dot  represents  the  position  of  the  REMUS 
vehicle  when  a  measurement  is  taken. 


Figure  17.  Bathymetry  data  points  overlaid  in  Google  Earth,  from  [21] 
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A  GoPro  camera  was  mounted  underneath  the  REMUS  vehiele  just  behind  the 
BlueView  micro-bathymetry  sonar.  Figure  18  is  a  snapshot  from  the  video  eollected 
during  the  mission.  The  black  cylindrical  object  at  the  top  of  the  image  is  the  forward- 
looking  sonar  /  miero-bathymetry  sonar  attaehment  on  the  REMUS  vehiele. 


Figure  18.  GoPro  image  from  SEATEST  11  mission 


Overall,  the  terrain  was  eomposed  of  rook  and  oorral  formations  with  intermittent 
sandy  bottomed  regions.  The  rock  and  corral  provided  noticeable  variability  in  seafloor 
depth.  Depth  ohanges  were  not  only  apparent  in  each  individual  sonar  image,  but  also  for 
the  dataset  as  a  whole.  All  56,491  data  points  collected  within  the  survey  area  are 
expressed  in  the  FTP  as  a  3-D  point  cloud  in  Figure  19. 
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Bathymetry  Point  Cloud 
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Figure  19.  3-D  bathymetry  point  cloud 

Figure  20  is  also  included  in  order  to  provide  a  different  perspective  of  the  data 
points  collected.  In  Figure  20,  the  REMUS ’s  entrance  and  exit  points  from  the  survey 
area  can  be  seen,  but  more  importantly  so  can  the  density  of  the  points  collected.  In 
particular,  associated  with  each  turning  point  is  a  gap  of  no  coverage.  It  is  important  to 
note  the  density  of  the  data  points  throughout  the  surveyed  area  since  the  linear 
interpolation  better  approximates  the  regions  that  have  more  data  points.  In  regions  where 
data  is  sparse,  the  linear  approximation  will  likely  be  a  poor  representation  of  the 
underlying  topography.  Passes  by  the  AUV  over  the  areas  interpolated  by  data  points 
distant  from  one  another  can  expect  a  poorer  correlation  between  the  sonar  information 
being  seen  and  what  the  interpolated  prior  map  shows.  This  will  negatively  impact  the 
overall  performance  of  the  TAN  solution. 

For  additional  perspective  on  the  dataset,  the  distance  between  each  ping  line  is 
approximately  two  meters  and  the  approximate  size  of  the  main  survey  area  is  20,000 
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LTP  Frame  -  North  (m) 


LTP  Frame  -  West  (m) 


square  meters.  The  distanee  between  pings  is  due  to  the  bathymetry  sonar  funetioning  at 
approximately  ~  0.5  Hz  throughout  the  mission  and  the  REMUS  having  a  mission 
defined  speed  of  2  knots. 


Bathymetry  Point  Cloud 


LTP  Frame  -  West  (m) 

Figure  20.  An  overhead  perspective  of  the  bathymetry  point  cloud 

In  order  to  provide  complete  coverage  of  the  area  surveyed,  the  bathymetry  points 
were  linearly  interpolated  to  produce  the  results  seen  in  Figure  21. 
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Interpolated  Bathymetry  Surface 


Figure  2 1 .  Interpolated  bathymetry  surfaee 


2,  Terrain  Aided  Navigation 

Once  the  prior  map  of  the  surveyed  area  was  constructed,  it  could  be  used  as  a 
navigational  aid.  Using  the  process  and  measurement  models  as  a  part  of  the  particle 
filter,  along  with  the  output  probability  density  function  from  the  correlation  step,  an 
estimate  of  the  vehicle  state  using  the  prior  bathymetry  map  was  calculated.  Specifically, 
after  the  REMUS  vehicle’s  first  pass  in  its  survey  area  it  has  already  accumulated  a 
significant  amount  of  positional  uncertainty  with  regard  to  the  INS  positional  estimate. 
An  example  of  the  magnitude  of  the  uncertainty  difference  between  the  vehicles  first  and 
second  passes  through  the  survey  area  is  depicted  in  Figure  22.  The  amount  of 
uncertainty  accumulated  through  the  first  pass  of  the  region  was  approximately  0.670 
meters  CEPR.  By  the  time  the  vehicle  passed  through  the  area  again,  the  positional 
uncertainty  was  approximately  8.068  meters  CEPR. 
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Figure  22.  Difference  in  uncertainty  of  INS  positional  estimation  between 

subsequent  passes  of  the  same  region 


Figure  23  is  an  example  of  the  two-dimensional  array  represented  as  an  intensity 
image  using  MATLAB’s  colormap.  Figure  24  includes  red  plus  sign  overlays  on  the 
image  that  represent  the  altitude  measurements  used  for  each  sonar  ping  (after 
thresholding). 
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SEATEST  II  Sonar  Image 
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Figure  23.  Sonar  image  collected  during  SEATEST  II 
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Figure  24.  SEATEST  II  sonar  image  after  thresholding  data  points 


In  Eigure  24,  it  can  be  seen  that  23  data  points  were  extracted.  The  represents  a 
swath  width  of  approximately  5.75  meters  orthogonal  to  the  longitudinal  direction  of  the 
AUV.  All  23  points  then  went  through  a  coordinate  transformation  based  upon  the  Euler 
angles  and  depth  of  the  vehicle  in  order  to  be  expressed  in  the  same  global  frame  as  the 
prior  bathymetry  map.  Eigure  25  shows  an  example  correlation  between  the  23  data 
points  expressed  in  the  global  frame  and  the  region  of  the  prior  map  encompassing  the 
vehicles  current  level  of  uncertainty. 
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Figure  25.  Correlation  probability  distribution  for  a  new  sonar  ping — p{yk  \  ) 

Figure  26  shows  a  sequenee  of  four  eorrelation  plots.  This  provides  a  snapshot  of 
the  multi-modality  that  typieally  is  assoeiated  with  sequential  pings  throughout  the 
dataset.  Each  plot  represents  a  probability  distribution  p{y,,  |  that  is  output  from  the 
correlation  step  and  used  in  the  recursive  Bayesian  filter. 


46 


Figure  26.  Correlation  probability  distributions  for  four  different  sonar  images 
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The  processing  of  each  sonar  image,  transformation  of  the  data  to  the  LTP 
reference  frame,  and  the  computation  of  a  probability  distribution  based  on  the 
correlation  for  the  ping  are  all  done  in  order  to  provide  the  measurement  update  for  the 
particle  filter.  The  measurement  update  and  the  process  model  for  the  propagation  of  the 
particles  are  the  two  main  components  of  the  particle  filter.  The  results  of  several  particle 
filtering  simulations  are  shown  below. 

The  particle  filter  was  first  tested  using  100  particles  over  the  course  of  54  micro¬ 
bathymetry  sonar  pings.  Figure  27  is  the  result  of  this  preliminary  test  of  the  particle  filter 
algorithm. 


PF  vs  INS  -  Second  Pass 
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Figure  27.  Particle  filter  results  on  first  leg  of  second  pass  of  the  survey  area 

Figure  27  is  indicative  of  the  rapid  convergence  of  the  particles  from  an  initial 
distribution  with  a  larger  standard  deviation,  to  a  much  more  tightly  grouped  set  of 
particles.  The  initial  standard  deviation  of  the  particles  is  3.994  meters,  while  eight  pings 
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later  the  standard  deviation  is  approximately  0.5  meters.  While  the  partieles  at  any  state 
other  than  the  initial  state  do  not  approximate  Gaussian  distributions,  the  standard 
deviation  is  still  used  here  in  order  to  provide  a  metric  as  to  the  spread  of  the  particles. 

With  the  support  of  preliminary  results,  subsequent  particle  fdter  testing  was 
completed  using  1,000  particles.  Figure  28  shows  the  results  using  1,000  particles  over  an 
entire  “navigate  rows”  mission  objective  through  the  survey  area. 


Particle  Filter  with  INS  Solution  in  Green 
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Figure  28.  Particle  filter  with  1,000  particles  compared  to  INS  estimation 

Again,  the  filter  can  be  seen  to  relatively  quickly  constrain  the  positional 
uncertainty  of  the  AUV  based  upon  the  measurement  and  process  updates.  A  GPS  fix 
was  taken  at  the  end  of  the  run  and  compared  to  the  INS  and  particle  filter  estimations. 
The  total  positional  uncertainty  at  the  end  of  the  run  was  15.627  meters.  The  GPS  fix  was 
acquired  using  seven  satellites  with  a  horizontal  error  of  approximately  two  meters.  The 
INS  solution  as  compared  to  the  GPS  fix  was  14.272  meters  away.  The  particle  filter 
estimation  as  compared  to  the  GPS  fix  was  8.863  meters  away. 
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There  are  several  error  sources  that  can  be  attributed  in  varying  degrees  to  the 
eventual  particle  filter  estimation  error  of  over  8  meters.  GPS  error,  both  from  the 
initialization  of  the  vehicle  at  the  start  of  the  mission  and  from  the  concluding  GPS  fix, 
could  certainly  account  for  a  significant  portion  of  the  error.  Additional  error  sources 
include  the  unaccounted  for  positional  uncertainty  in  creating  the  prior  bathymetry  map, 
the  linear  interpolation  of  the  prior  map,  and  the  particle  filtering  method.  Within  particle 
filtering,  there  can  be  a  couple  sources  of  error  ranging  from  the  selection  of  too  few 
particles  to  properly  approximate  the  probability  density  to  the  impact  of  various  particle 
resampling  techniques.  Further  literature  research  unveiled  a  detrimental  effect  of 
implementing  a  SIR  particle  filter  on  a  vehicle  with  a  high  precision  INS  called  “sample 
impoverishment”  [2],  [22],  and  [23].  A  process  model  with  small  process  noise,  like  that 
of  the  REMUS  INS  system,  can  be  susceptible  to  a  convergence  on  a  false  estimate.  The 
particles  are  then  propagated  with  small  process  noise  and  thus  remain  tightly  clustered 
around  a  false  estimate  with  little  chance  of  recovery.  This  issue  can  be  the  result  of  a 
terrain  change  or  artifacts  in  the  prior  map  generation  [22],  or  from  premature 
convergence  on  a  false  estimate  [23]. 

In  order  to  address  the  concerns  of  sample  impoverishment  due  to  the  small 
process  noise  associated  with  the  REMUS  INS,  the  particle  filter  was  re-initialized 
shortly  prior  to  surfacing  for  a  GPS  fix.  Eigure  29  is  the  result  from  this  simulation. 
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1000  Particles  -  Initialized  shortly  prior  to  GPS  fix 


LTP  -  West  (m) 

Figure  29.  Particle  filter  with  1,000  particles  prior  to  GPS  fix 


It  can  be  seen  in  Figure  29  how  the  particle  filter  did  not  immediately  converge  to 
a  single,  tightly  group  position  estimate  but  instead  supported  multiple  hypotheses  as  to 
the  location  of  the  vehicle  based  upon  the  correlations  being  performed.  Figure  30  is 
provided  in  order  to  clearly  visualize  the  GPS  positional  update  along  with  the  final 
iteration  of  the  particle  filter  using  1000  particles.  While  there  clearly  are  multiple 
hypothesis  as  to  the  position  of  the  AUV,  the  red  “x”  indicates  the  position  of  the  best 
correlated  particle.  The  best  correlated  particle  is  approximately  2.846  meters  away  from 
the  GPS  updated  navigation  estimate. 
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1000  Particles  -  Final  iteration  including  GPS  update 
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Figure  30.  Final  iteration  of  particle  filter  using  1000  particles 


The  results  from  this  simulation  indicate  that  the  relatively  poor  results  from  the 
testing  of  the  particle  filter  on  the  full  mission  were  substantially  due  to  a  false  or 
premature  convergence  upon  an  estimate.  Initializing  the  particle  filter  closer  to  the  GPS 
fix  produced  significantly  more  accurate  results.  Further  discussions  on  techniques  that 
mitigate  the  chance  of  false  convergence  and  sample  impoverishment  are  discussed  in  the 
section  on  future  work. 


52 


VI.  CONCLUSIONS 


A,  PERFORMANCE  ASSESSMENT 

This  thesis  is  the  first  to  address  terrain-aided  navigation  for  a  small,  man- 
portable  AUV.  It  included  the  entirety  of  the  TAN  process  from  building  the  prior 
bathymetry  map  as  a  navigational  aid  to  the  implementation  and  analysis  of  a  particle 
filter  for  AUV  position  estimation.  The  combination  of  the  ultra-high-resolution 
downward-looking  sonar,  INS  and  particle  filter  approach  showed  great  promise  for 
accurate  underwater  navigation. 

That  said  there  are  many  areas  for  improvement.  They  can  be  categorized  as 
follows: 

1 .  Map  building 

a.  Data  interpolation 

2.  Image  processing 

3.  Particle  filter  implementation 

4.  Real-time  implementation 

1,  Map  Building 

One  of  the  most  significant  challenges  in  building  an  accurate  bathymetry  map  is 
accounting  for  the  inherent,  growing  uncertainty  associated  with  the  INS  of  the  AUV. 
The  problem  poses  quite  a  conundrum.  Through  TAN  methods,  one  wishes  to  localize 
the  position  of  the  vehicle  through  the  use  of  a  prior  map  due  to  accumulating  uncertainty 
with  regards  to  position.  However,  the  prior  map  is  built  using  the  INS  solution,  with  its 
growing  uncertainty,  in  the  first  place.  Therefore,  additional  research  is  recommended 
into  building  a  more  accurate  prior  bathymetry  map.  A  relatively  simplistic  fix  to  the  bulk 
of  this  issue  would  be  to  survey  an  area  using  an  LBL  system.  Although  this  would  not 
eliminate  the  uncertainty  with  regards  to  the  vehicle  and  thus  the  map,  it  would  constrain 
it.  Other  potential  methods  of  building  a  more  accurate  map  could  be  the  use  of  a  quad¬ 
tree  data  structure  that  stores  data  different  resolution  grids  based  upon  the  certainty  of 
the  estimate  or  through  ensuring  consistency  of  the  map  by  feature  based  correction 
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methods.  This  would  entail  a  similar  eorrelation  technique  as  proposed  above,  but  this 
correlation  would  be  utilized  in  ensuring  consistency  with  regards  to  the  locations  of 
features  in  the  map.  Overall,  the  vehicular  positional  uncertainty  while  building  a 
bathymetry  map  should  be  taken  into  account,  and  methods  for  doing  so  are  currently 
being  researched.  The  fact  that  this  was  not  taken  into  account  in  building  the  prior 
bathymetry  map  is  a  primary  suspect  for  the  poorer  than  expected  accuracy  of  the  particle 
filter. 


a.  Data  Interpolation 

As  mentioned  in  the  data  interpolation  paragraph,  the  TriScatteredInterp 
MATLAB  function  was  used  in  order  to  perform  a  simple  linear  interpolation  through  all 
of  the  data  points.  A  potentially  more  sophisticated  method  of  interpolation  (e.g., 
Kriging,  Gaussian  random  fields  or  compressive  sampling)  takes  into  consideration  the 
two-dimensional  signal  and  the  variance  of  the  data  may  lead  to  a  more  accurate 
underlying  map. 

2.  Image  Processing 

The  sonar  image  processing  algorithm  utilized  on  this  particular  dataset  worked 
efficiently  and  effectively.  However,  the  algorithm  is  not  robust  to  any  significant 
changes  in  the  environment.  Poor  performance  could  be  expected  in  more  dynamic, 
cluttered  environments.  In  particular,  testing  in  a  harbor  or  kelp  field  would  necessitate  a 
more  advanced  and  robust  image  processing  algorithm.  As  a  start,  an  adaptive  threshold 
for  identifying  returns  associated  with  the  sea  floor  in  various  different  environments 
with  different  bottom  compositions  would  be  useful.  Additional  work  may  be  done  to 
mitigate  the  effects  of  any  noise  that  may  appear  in  a  dataset  (noise  was  not  a  significant 
issue  in  the  dataset  collected).  Nonetheless,  the  resolution  of  the  data  extracted  from  the 
image  is  only  constrained  by  the  fundamental  pixel  to  meter  ratio  of  0.023052  and 
therefore  the  resolution  could  be  increased  at  any  point  if  it  would  be  deemed  worth  the 
extra  computational  burden.  More  importantly,  some  changes  on  the  image  processing 
algorithm  itself  may  be  advised. 
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3,  Particle  Filter  Resampling  Technique 

As  mentioned,  the  SIR  particle  filter  implementation  suffers  from  sample 
impoverishment.  The  sample  impoverishment  is  largely  attributed  to  the  small  process 
noise  associated  with  the  high-grade  INS  of  the  REMUS  vehicle.  A  few  methods  that 
help  counteract  or  negate  this  effect  are  discussed  in  [2],  [22],  and  [23].  In  particular,  the 
Rao-Blackwellization  of  the  particle  filter  as  described  in  [2]  is  the  recommended  course 
of  action  for  future  work.  Not  only  will  Rao-Blackwellization  help  combat  the  effects  of 
small  process  noise  on  the  particle  filter  solution,  but  it  will  also  reduce  computational 
complexity  and  should  improve  accuracy  [2],  [5].  Other  potential  solutions  include  re¬ 
initializing  the  particle  filter  with  a  much  large  covariance  when  an  error  in  estimation 
has  been  detected  [22],  using  a  genetic  algorithm  [23],  or  through  the  use  of  a  different 
particle  filter  resampling  technique  such  as  the  resample-move  algorithm  described  in 
[24]  or  the  regularization  method  described  in  [25]. 

4,  Real-time  Implementation  on  REMUS  100  AUV 

Perhaps  most  importantly,  the  end  goal  of  this  line  of  research  is  to  implement  a 
real-time  system  on  CAVR’s  REMUS  100  AUV.  Concerns  as  to  the  long-term  accuracy 
of  the  particle  filtering  solution  should  be  addressed  prior  to  fully  trusting  the  navigation 
solution  provided  by  the  particle  filter.  Notwithstanding  these  concerns,  the  work 
provided  thus  far  is  suited  to  experimental  testing  on  the  REMUS  vehicle.  Real-time 
implementation  would  be  the  next  major  step  in  advancing  TAN  for  an  AUV. 
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